/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file amcl_node.h
 *  功能: amcl定位
 *
 *                                                                         *
 ***************************************************************************/
#ifndef AMCL_NODE_H
#define AMCL_NODE_H


#include <algorithm>
#include <vector>
#include <map>
#include <cmath>

#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>

// Signal handling
#include <signal.h>

#include "amcl/map/map.h"
#include "amcl/pf/pf.h"
#include "amcl/sensors/amcl_odom.h"
#include "amcl/sensors/amcl_laser.h"

#include "ros/assert.h"

// roscpp
#include "ros/ros.h"

// Messages that I need
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Pose.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/SetMap.h"
#include "std_srvs/Empty.h"

// For transform support
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "tf/tf.h"
#include "message_filters/subscriber.h"

// Dynamic_reconfigure
#include "dynamic_reconfigure/server.h"
#include "amcl/AMCLConfig.h"

// Allows AMCL to run from bag file
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <amcl/AmclExtension.h>
#include "amclconfig.h"
#include "globalDefine.h"
#include "GeneralDefine.h"
#include <QMutex>
namespace KDL{
class Frame;
}

#define NEW_UNIFORM_SAMPLING 1

using namespace amcl;

// Pose hypothesis
typedef struct
{
  // Total weight (weights sum to 1)
  double weight;

  // Mean of pose esimate
  pf_vector_t pf_pose_mean;

  // Covariance of pose estimate
  pf_matrix_t pf_pose_cov;

} amcl_hyp_t;



class FilteringAlgorithm;
/**
 * @brief The AmclNode class 实现机器人的位置计算.
 */
class AmclNode
{
  public:
    AmclNode(AmclConfig* configParameterIn);
    ~AmclNode();
    public:
    bool isAmclPositionInitialOk();
    int getAmclDebugInfo(AmclDebugInfo &debugInfoOut);
    int notifyReflectPositionInfo(PositionInfo vehiclePositionIn);
    int setAmclPara(struct AmclDebugPara paraIn);
    int getAmclPara(struct AmclDebugPara &paraOut);
    int forceInitialAgvPosition(int calculateTime);
    int getVehiclePositionFrom_amcl(PositionInfo &positionInfoOut);//20ms实时周期被调用，通过矩阵计算位置
    int calculateAgvPosition_byOdom(PositionInfo &vehiclePositionInfoOut);
    /**
     * @brief setLaserLinkInBaseLink
     * @param x unit is m
     * @param y
     * @param z
     * @param roll unit is rad
     * @param pitch
     * @param yaw
     * @return
     */
    int setLaserLinkInBaseLink(double x,double y,double z,double roll,double pitch,double yaw);
    int setWheelOdometerData_noRos( struct OdemeterData odemeterDataIn);
    int getInitialPose(double &xOut,double &yOut,double &rOut);
    int setAmclIsCalculateLaser(bool isCalculateLaserIn);//设置是否计算激光数据
    int setLaserSensorData(const sensor_msgs::LaserScanConstPtr &laserSensorDataIn);//通过函数传递激光数据，不使用ros消息。
    int requestCountPlus();//非实时线程调用
//    每个周期迭代至少一次计算，单个周期的迭代次数，不超过最大值，一直到匹配评分没有显著增加为止。
    int getRequestCount();//成功计算ａｍｃｌ位置异常判断。不能作为激光数据丢失的依据，可能某一次激光要处理很久。
    int setNoCheckCount(int countIn);//因为加载地图堵塞时间太长，需要屏蔽保护一段时间
    int getLaserRequestCount();//激光数据请求异常判断
    int setVehicleVelocityFeedback(double lineSpeed,double rotateSpeed);////根据速度自动修改距离和计算次数
    int setWorkStatus(bool isEnable);
    int setUpdatePartialEveryTime(bool isUpdateIn);
    int getPositionCertaintyFactor(double &certaintyFactorOut, float &weight_out, std::vector<float> &pos_cov_out);
    /**
     * @brief getTimeConsumed
     * @param minConsumedOut unit is us
     * @param currentConsumedOut  unit is us
     * @param maxConsumedOut  unit is us
     * @return
     */
    int getTimeConsumed(double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int resetRosMeasureTime();

    /**
     * @brief Uses TF and LaserScan messages from bag file to drive AMCL instead
     */
    void runFromBag(const std::string &in_bag_fn);

    int process();
    void savePoseToServer();
    /**
     * @brief isLaserReceiveOkStatus //amclError　AmclNode节点原来的激光超时接收判断，超时10~15s
     * @return
     */
    bool isLaserReceiveOkStatus();
    bool isPfOkStatus();
    int setPartialNum(int minNum, int maxNum, int maxBeams);

    int setDeltaDistanceUpdate(double deltaDistance);//和速度关联起来
    int setDeltaAngleUpdate(double deltaAngle);//和速度关联起来

private:
    void sendOdomToMapTf(VehiclePosition positionIn);
    /**
     * @brief angleRotationJudgement 考虑哪种角度旋转方法最近
     * @param targetAngle
     * @param initialAngle
     * @return
     */
    double angleRotationJudgement(double targetAngle,double initialAngle);//到目标姿态的最小路径转角

    int calculateCorrectPosition(PositionInfo reflectPositionIn,double &xOut,double &yOut,double &rOut);

    int calculateOffsetMatrix(VehiclePosition agvInMapPositionIn, KDL::Frame *frame_baselink_in_odomIn,
                                              KDL::Frame *frame_odom_in_mapOut);
    int calculateOffsetMatrix_tf(tf::Stamped<tf::Pose> odom_to_mapIn,KDL::Frame *frame_odom_in_mapOut);
    int checkMatchScoreAndChangeOdom();// used
    int resetParameter(AmclConfig* configParameterIn);
    int laserDataCalculate(const sensor_msgs::LaserScanConstPtr& laser_scan);//not used
    int laserDataCalculate_multicalculate(const sensor_msgs::LaserScanConstPtr &laser_scan, bool &isReSetCloudOut);
    int laserDataCalculate_multicalculate_once(const sensor_msgs::LaserScanConstPtr &laser_scan,
                                                         int calculateCount);
    int laserDataCalculate_internalICP(const sensor_msgs::LaserScanConstPtr &laser_scan,
                                       int calculateTimesIn, bool &isReSetCloudOut);

    // Use a child class to get access to tf2::Buffer class inside of tf_
    struct TransformListenerWrapper : public tf::TransformListener
    {
      inline tf2_ros::Buffer &getBuffer() {return tf2_buffer_;}
    };

    // Pose-generating function used to uniformly distribute particles over
    // the map
    static pf_vector_t uniformPoseGenerator(void* arg);
#if NEW_UNIFORM_SAMPLING
    static std::vector<std::pair<int,int> > free_space_indices;
#endif
    // Callbacks
    bool globalLocalizationCallback(std_srvs::Empty::Request& req,
                                    std_srvs::Empty::Response& res);
    bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
                                    std_srvs::Empty::Response& res);
    bool setMapCallback(nav_msgs::SetMap::Request& req,
                        nav_msgs::SetMap::Response& res);

    void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);
    void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
    void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);
    void setCorrectPose(VehiclePosition positionIn,bool pf_inital=false);
    void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);

    void handleMapMessage(const nav_msgs::OccupancyGrid& msg);
    void freeMapDependentMemory();
    map_t* convertMap( const nav_msgs::OccupancyGrid& map_msg );
    void updatePoseFromServer();
    void applyInitialPose(bool pf_initIn=false);

    double getYaw(tf::Pose& t);

private:
    tf::TransformBroadcaster* tfb_;

    TransformListenerWrapper* tf_;

    bool sent_first_transform_;

    tf::Transform latest_tf_;
    bool latest_tf_valid_;

    //parameter for what odom to use
    std::string odom_frame_id_;

    //paramater to store latest odom pose
    tf::Stamped<tf::Pose> latest_odom_pose_;

    //parameter for what base to use
    std::string base_frame_id_;
    std::string global_frame_id_;

    bool use_map_topic_;
    bool first_map_only_;

    ros::Duration gui_publish_period;
    ros::Time save_pose_last_time;
    ros::Duration save_pose_period;

    geometry_msgs::PoseWithCovarianceStamped last_published_pose;//isCorrectByReflect

    map_t* map_;
    char* mapdata;
    int sx, sy;
    double resolution;

    message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
    tf::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
    ros::Subscriber initial_pose_sub_;
    std::vector< AMCLLaser* > lasers_;
    std::vector< bool > lasers_update_;
    std::map< std::string, int > frame_to_laser_;

    // Particle filter
    pf_t *pf_;
    double pf_err_, pf_z_;
    bool pf_init_;
    pf_vector_t pf_odom_pose_;//最后一次的里程计位置
    double d_thresh_, a_thresh_;
    int resample_interval_;
    int resample_count_;
    double laser_min_range_;
    double laser_max_range_;

    //Nomotion update control
    bool m_force_update;  // used to temporarily let amcl update samples even when no motion occurs...

    AMCLOdom* odom_;
    AMCLLaser* laser_;

    ros::Duration cloud_pub_interval;
    ros::Time last_cloud_pub_time;

    // For slowing play-back when reading directly from a bag file
    ros::WallDuration bag_scan_period_;

    void requestMap();

    // Helper to get odometric pose from transform system
    bool getOdomPose(tf::Stamped<tf::Pose>& pose,
                     double& x, double& y, double& yaw,
                     const ros::Time& t, const std::string& f);

    //time for tolerance on the published transform,
    //basically defines how long a map->odom transform is good for
    ros::Duration transform_tolerance_;

    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;
    ros::Publisher pose_pub_;
    ros::Publisher mapMatchScore_pub_;//modify by hualei
    AmclExtension mathWightToPub;//modify by hualei
    ros::Publisher particlecloud_pub_;
    ros::ServiceServer global_loc_srv_;
    ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
    ros::ServiceServer set_map_srv_;
    ros::Subscriber initial_pose_sub_old_;
    ros::Subscriber map_sub_;


    amcl_hyp_t* initial_pose_hyp_;
    bool first_map_received_;
    bool first_reconfigure_call_;

    boost::recursive_mutex configuration_mutex_;
    dynamic_reconfigure::Server<amcl::AMCLConfig> *dsrv_;
    amcl::AMCLConfig default_config_;
    ros::Timer check_laser_timer_;

    int max_beams_, min_particles_, max_particles_;
    double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
    double alpha_slow_, alpha_fast_;
    double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
  //beam skip related params
    bool do_beamskip_;
    double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;
    double laser_likelihood_max_dist_;
    odom_model_t odom_model_type_;
    double init_pose_[3];
    double init_cov_[3];
    laser_model_t laser_model_type_;
    bool tf_broadcast_;

    void reconfigureCB(amcl::AMCLConfig &config, uint32_t level);

    ros::Time last_laser_received_ts_;
    ros::Duration laser_check_interval_;
    void checkLaserReceived(const ros::TimerEvent& event);

    //----------------------------------hualei
    bool isLaserReceiveOk;//hualei
    bool isPfOk;//hualei
    int isUpdatePartialEveryTime;//hualei
    int calculateTimesForLaser;//hualei
    double positionCertaintyFactor;//hualei
    double minConsumed;
    double currentConsumed;
    double maxConsumed;
    pf_vector_t current_pose_mean;//位置估计
    bool isEnableToWork;

    float socre_weight;
    float socre_pos_cov[9];

    AmclConfig *configParameter;
    int extRequestCount;//成功计算ａｍｃｌ位置异常判断
    int noCheckCount;//屏蔽检查计数
    int laserRequestCount;//激光数据请求异常判断
    sensor_msgs::LaserScanConstPtr laser_scan_buffer;
    bool isLaserReadyForCalculate;
    bool isUseRosLaserMsg;//是否使用ros激光消息（否则用内部函数传递的）
    double goodMatchScore;
    double badMatchScore;
    double newOdom_alpha_badMatchScore;
    bool isCalculateLaser;

    QMutex usingMutex;
    KDL::Frame *frame_baselink_in_laserlink;
    KDL::Frame *frame_laserlink_in_baselink;

    OdemeterData odemeterData;//里程计信息
    KDL::Frame *frame_baselink_in_odom;//轮子里程计
    KDL::Frame *frame_baselink_in_odom_old;//轮子里程计,计算用
    KDL::Frame *frame_odom_in_map;//里程计偏差
//    KDL::Frame *frame_correct;//修正偏差
//    PositionInfo vehiclePosition;
    int initialAgvPositionCount;//初始化agv位置时，强制更新次数

    FilteringAlgorithm* filtering_position_x;
    FilteringAlgorithm* filtering_position_y;
    FilteringAlgorithm* filtering_position_r;

    int initialPositionCount;
    int odomOffsetFilterCount;

    bool isCorrectByReflect;
    double qualifyReflectMinCertaintyScore;
    int correctFilterCount;
    double correctAllowMaxDistanceOffset;
    double correctAllowMaxAngleOffset;

    PositionInfo amclPosition;
    PositionInfo reflectPosition;

    AmclDebugInfo debugInfo;
    int debugFlag;
    bool force_publication;
};

#endif // AMCL_NODE_H
